/**
 * @file data_fusion.c
 * @brief 数据融合模块实现
 * @version 2.0
 * @date 2023-12-10
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>
#include <time.h>
#include "data_fusion.h"
#include "app_control.h"
#include "system_utils.h"
#include "ice_calculation.h"

// 异常检测标志位
#define ANOMALY_SENSOR_JUMP      0x01 // 传感器数据跳变
#define ANOMALY_VISUAL_JUMP      0x02 // 视觉数据跳变
#define ANOMALY_STEREO_JUMP      0x04 // 立体视觉数据跳变
#define ANOMALY_DATA_CONFLICT    0x08 // 多源数据冲突
#define ANOMALY_TIMEOUT          0x10 // 数据超时
#define ANOMALY_WEATHER_EXTREME  0x20 // 极端天气条件
#define ANOMALY_READING_INVALID  0x40 // 无效读数
#define ANOMALY_SENSOR_TIMEOUT   0x80 // 传感器数据超时

// 数据滑动窗口大小
#define SLIDING_WINDOW_SIZE 10

// 默认融合参数
#define DEFAULT_PROCESS_NOISE 0.01f      // 卡尔曼滤波器过程噪声（Q）
#define DEFAULT_MEASUREMENT_NOISE 0.1f   // 卡尔曼滤波器测量噪声（R）
#define DEFAULT_ANOMALY_THRESHOLD 3.0f   // 异常检测阈值（标准差倍数）

// 时间阈值定义（毫秒）
#define DATA_TIMEOUT_MS 5000    // 数据超时阈值
#define STARTUP_PERIOD_MS 30000 // 启动期（此时间内权重稳定增长）
#define MAX_DATA_AGE 5000      // 数据超时阈值（毫秒）

// 全局变量引用
extern uint64_t g_program_start_time; // 来自app_control.c

// 全局数据质量评估结构
static struct {
    DataSourceQuality sensor_quality;  // 传感器数据质量
    DataSourceQuality visual_quality;  // 视觉数据质量
    DataSourceQuality stereo_quality;  // 立体视觉数据质量
    DataSourceQuality weather_quality; // 气象数据质量
    
    // 滑动窗口数据缓存
    float sensor_window[SLIDING_WINDOW_SIZE];
    float visual_window[SLIDING_WINDOW_SIZE];
    float stereo_window[SLIDING_WINDOW_SIZE];
    int window_index;
    bool window_filled;
    
    // 卡尔曼滤波器状态
    KalmanFilter ice_thickness_filter;
    
    // 历史数据记录
    float last_visual_thickness;
    float last_stereo_thickness;
    float last_sensor_tension;
    float last_temperature;
    float last_humidity;
    float last_wind_speed;
    uint64_t last_update_time;
    
    // 融合参数
    float process_noise;       // 过程噪声
    float measurement_noise;   // 测量噪声
    float anomaly_threshold;   // 异常检测阈值
    
    // 模块状态
    bool initialized;
} g_fusion_state = {0};

// 数据质量描述文本
static const char* data_quality_descriptions[] = {
    "无效",
    "低质量",
    "中等质量",
    "高质量",
    "极高质量"
};

// 函数前向声明
static void init_data_source_quality(DataSourceQuality* quality);
static void init_kalman_filter(KalmanFilter* filter, float process_noise, float measurement_noise);
static void init_sliding_window(void);
static float apply_kalman_filter(KalmanFilter* filter, float measurement);
static void update_data_quality(DataSourceQuality* quality, float current_value, float prev_value, uint64_t current_time);
static float calculate_variance(float* data, int count);
static bool detect_anomaly(float current_value, float prev_value, float variance, float threshold);
static float* get_data_window(int source_type);
static void update_sliding_window(int source_type, float value);
static float calculate_median(float* data, int count);
static float calculate_data_confidence(const DataSourceQuality* quality);
static float calculate_fusion_weight(const DataSourceQuality* quality, uint64_t startup_time, uint64_t current_time);
static bool is_weather_extreme(float temperature, float wind_speed);
static DataQuality determine_data_quality(float confidence, int consecutive_valid);
static float data_fusion_calculate_ice_growth_rate(float temperature, float humidity, float wind_speed);

// 添加缺少的前向声明，这些函数在被调用之前需要声明
static void calculate_ice_metrics(float thickness, float cable_diameter, float ice_density,
                              float *out_volume, float *out_weight);
static float calculate_ice_tension(float ice_weight, float span);
static RiskLevel calculate_risk_level(float thickness, float ice_weight, float wind_speed);

/**
 * @brief 初始化数据源质量结构
 * 
 * @param quality 质量结构指针
 */
static void init_data_source_quality(DataSourceQuality* quality) {
    if (!quality) return;
    
    quality->quality = DATA_QUALITY_INVALID;
    quality->confidence = 0.0f;
    quality->last_update_time = 0;
    quality->stability = 0.0f;
    quality->variance = FLT_MAX; // 初始设为最大值
    quality->consecutive_valid = 0;
    quality->consecutive_invalid = 0;
}

/**
 * @brief 初始化卡尔曼滤波器
 * 
 * @param filter 卡尔曼滤波器结构指针
 * @param process_noise 过程噪声
 * @param measurement_noise 测量噪声
 */
static void init_kalman_filter(KalmanFilter* filter, float process_noise, float measurement_noise) {
    if (!filter) return;
    
    filter->x = 0.0f;             // 初始状态估计
    filter->P = 1.0f;             // 初始协方差
    filter->Q = process_noise;    // 过程噪声
    filter->R = measurement_noise;// 测量噪声
    filter->K = 0.0f;             // 初始卡尔曼增益
    filter->initialized = false;
}

/**
 * @brief 初始化滑动窗口
 */
static void init_sliding_window(void) {
    memset(g_fusion_state.sensor_window, 0, sizeof(g_fusion_state.sensor_window));
    memset(g_fusion_state.visual_window, 0, sizeof(g_fusion_state.visual_window));
    memset(g_fusion_state.stereo_window, 0, sizeof(g_fusion_state.stereo_window));
    g_fusion_state.window_index = 0;
    g_fusion_state.window_filled = false;
}

/**
 * @brief 应用卡尔曼滤波器进行数据平滑
 * 
 * @param filter 卡尔曼滤波器结构指针
 * @param measurement 测量值
 * @return float 滤波后的估计值
 */
static float apply_kalman_filter(KalmanFilter* filter, float measurement) {
    if (!filter) return measurement;
    
    // 第一次使用滤波器，直接初始化状态
    if (!filter->initialized) {
        filter->x = measurement;
        filter->P = 1.0f;
        filter->initialized = true;
        return filter->x;
    }
    
    // 预测步骤
    // x(k|k-1) = x(k-1|k-1), 简化模型，无状态转移
    // P(k|k-1) = P(k-1|k-1) + Q
    filter->P = filter->P + filter->Q;
    
    // 更新步骤
    // K(k) = P(k|k-1) / (P(k|k-1) + R)
    filter->K = filter->P / (filter->P + filter->R);
    // x(k|k) = x(k|k-1) + K(k) * (z(k) - x(k|k-1))
    filter->x = filter->x + filter->K * (measurement - filter->x);
    // P(k|k) = (1 - K(k)) * P(k|k-1)
    filter->P = (1 - filter->K) * filter->P;
    
    return filter->x;
}

/**
 * @brief 更新数据源质量评估
 * 
 * @param quality 质量结构指针
 * @param current_value 当前数据值
 * @param prev_value 前一次数据值
 * @param current_time 当前时间戳
 */
static void update_data_quality(DataSourceQuality* quality, float current_value, float prev_value, uint64_t current_time) {
    if (!quality) return;
    
    uint64_t time_diff = 0;
    if (quality->last_update_time > 0) {
        // 计算时间差（毫秒）
        time_diff = current_time - quality->last_update_time;
    }
    
    // 检查数据有效性（这里简单检查非负值和非NaN）
    bool is_valid = current_value >= 0.0f && !isnan(current_value);
    
    // 更新连续有效/无效计数
    if (is_valid) {
        quality->consecutive_valid++;
        quality->consecutive_invalid = 0;
    } else {
        quality->consecutive_invalid++;
        quality->consecutive_valid = 0;
    }
    
    // 计算变化率
    float change_rate = 0.0f;
    if (is_valid && prev_value >= 0.0f && time_diff > 0) {
        change_rate = fabsf(current_value - prev_value) / time_diff;
    }
    
    // 稳定性指标更新 - 结合变化率和连续有效计数
    if (is_valid) {
        // 较小的变化率导致较高的稳定性
        float stability_change = (change_rate > 0.001f) ? (0.001f / change_rate) : 1.0f;
        
        // 指数平滑更新稳定性
        if (quality->stability <= 0.0f) {
            quality->stability = stability_change;
        } else {
            quality->stability = 0.8f * quality->stability + 0.2f * stability_change;
        }
        
        // 约束稳定性在[0,1]范围内
        if (quality->stability > 1.0f) quality->stability = 1.0f;
        if (quality->stability < 0.0f) quality->stability = 0.0f;
    } else {
        // 无效数据降低稳定性
        quality->stability *= 0.9f;
    }
    
    // 更新方差 - 方差越小，数据质量越高
    if (is_valid) {
        if (quality->variance == FLT_MAX) {
            quality->variance = 0.1f; // 初始设置为适中值
        } else {
            // 使用指数平滑更新方差
            float instant_variance = (current_value - prev_value) * (current_value - prev_value);
            quality->variance = 0.9f * quality->variance + 0.1f * instant_variance;
        }
    }
    
    // 更新置信度 - 基于稳定性和连续有效计数
    quality->confidence = calculate_data_confidence(quality);
    
    // 更新质量等级
    quality->quality = determine_data_quality(quality->confidence, quality->consecutive_valid);
    
    // 更新最后更新时间
    quality->last_update_time = current_time;
}

/**
 * @brief 计算数据方差
 * 
 * @param data 数据数组
 * @param count 数据数量
 * @return float 方差
 */
static float calculate_variance(float* data, int count) {
    if (!data || count <= 1) return 0.0f;
    
    // 计算平均值
    float sum = 0.0f;
    int valid_count = 0;
    
    for (int i = 0; i < count; i++) {
        if (data[i] >= 0.0f) { // 仅计算有效数据
            sum += data[i];
            valid_count++;
        }
    }
    
    if (valid_count <= 1) return 0.0f;
    
    float mean = sum / valid_count;
    
    // 计算方差
    float variance_sum = 0.0f;
    for (int i = 0; i < count; i++) {
        if (data[i] >= 0.0f) { // 仅计算有效数据
            float diff = data[i] - mean;
            variance_sum += diff * diff;
        }
    }
    
    return variance_sum / (valid_count - 1);
}

/**
 * @brief 检测数据异常
 * 
 * @param current_value 当前值
 * @param prev_value 前一值
 * @param variance 历史方差
 * @param threshold 异常阈值（标准差倍数）
 * @return true 检测到异常
 * @return false 数据正常
 */
static bool detect_anomaly(float current_value, float prev_value, float variance, float threshold) {
    // 如果没有前值或方差无效，无法检测
    if (prev_value < 0.0f || variance <= 0.0f) return false;
    
    // 计算当前值与前值的差距（变化量）
    float diff = fabsf(current_value - prev_value);
    
    // 计算标准差
    float std_dev = sqrtf(variance);
    
    // 如果变化量超过阈值倍标准差，判定为异常
    return diff > threshold * std_dev;
}

/**
 * @brief 获取指定数据源的滑动窗口
 * 
 * @param source_type 数据源类型（使用数据源可用性标志位）
 * @return float* 数据窗口指针，如果类型无效则返回NULL
 */
static float* get_data_window(int source_type) {
    switch (source_type) {
        case SENSOR_DATA_AVAILABLE:
            return g_fusion_state.sensor_window;
        case VISUAL_DATA_AVAILABLE:
            return g_fusion_state.visual_window;
        case STEREO_DATA_AVAILABLE:
            return g_fusion_state.stereo_window;
        default:
            return NULL;
    }
}

/**
 * @brief 更新滑动窗口
 * 
 * @param source_type 数据源类型
 * @param value 数据值
 */
static void update_sliding_window(int source_type, float value) {
    float* window = get_data_window(source_type);
    if (!window) return;
    
    // 更新窗口中的值
    window[g_fusion_state.window_index] = value;
    
    // 更新窗口索引和状态
    g_fusion_state.window_index = (g_fusion_state.window_index + 1) % SLIDING_WINDOW_SIZE;
    if (g_fusion_state.window_index == 0) {
        g_fusion_state.window_filled = true;
    }
}

/**
 * @brief 计算中位数（抗异常值）
 * 
 * @param data 数据数组
 * @param count 数据数量
 * @return float 中位数
 */
static float calculate_median(float* data, int count) {
    if (!data || count <= 0) return 0.0f;
    
    // 创建临时数组进行排序
    float* temp = (float*)malloc(count * sizeof(float));
    if (!temp) return 0.0f;
    
    // 复制数据
    memcpy(temp, data, count * sizeof(float));
    
    // 冒泡排序（对于小数组足够）
    for (int i = 0; i < count - 1; i++) {
        for (int j = 0; j < count - i - 1; j++) {
            if (temp[j] > temp[j + 1]) {
                float swap = temp[j];
                temp[j] = temp[j + 1];
                temp[j + 1] = swap;
            }
        }
    }
    
    // 计算中位数
    float median;
    if (count % 2 == 0) {
        median = (temp[count/2 - 1] + temp[count/2]) / 2.0f;
    } else {
        median = temp[count/2];
    }
    
    free(temp);
    return median;
}

/**
 * @brief 计算数据置信度
 * 
 * @param quality 数据源质量结构
 * @return float 置信度（0.0-1.0）
 */
static float calculate_data_confidence(const DataSourceQuality* quality) {
    if (!quality) return 0.0f;
    
    // 基于稳定性和连续有效计数计算置信度
    float stability_weight = 0.6f;  // 稳定性权重
    float count_weight = 0.4f;      // 连续有效计数权重
    
    // 将连续有效计数归一化（最多考虑20次连续有效）
    float count_factor = fminf((float)quality->consecutive_valid / 20.0f, 1.0f);
    
    // 计算加权置信度
    float confidence = stability_weight * quality->stability + count_weight * count_factor;
    
    // 确保置信度在[0,1]范围内
    return fmaxf(0.0f, fminf(confidence, 1.0f));
}

/**
 * @brief 计算融合权重
 * 
 * @param quality 数据源质量结构
 * @param startup_time 系统启动时间
 * @param current_time 当前时间
 * @return float 融合权重（0.0-1.0）
 */
static float calculate_fusion_weight(const DataSourceQuality* quality, uint64_t startup_time, uint64_t current_time) {
    if (!quality) return 0.0f;
    
    // 基础权重基于置信度
    float base_weight = quality->confidence;
    
    // 在启动期间，权重逐渐增加到最大值
    if (current_time - startup_time < STARTUP_PERIOD_MS) {
        float startup_factor = (float)(current_time - startup_time) / STARTUP_PERIOD_MS;
        base_weight *= startup_factor;
    }
    
    // 如果数据质量无效，权重为0
    if (quality->quality == DATA_QUALITY_INVALID) {
        return 0.0f;
    }
    
    // 基于质量级别调整权重
    float quality_factor = 0.0f;
    switch (quality->quality) {
        case DATA_QUALITY_LOW:
            quality_factor = 0.3f;
            break;
        case DATA_QUALITY_MEDIUM:
            quality_factor = 0.6f;
            break;
        case DATA_QUALITY_HIGH:
            quality_factor = 0.9f;
            break;
        case DATA_QUALITY_EXCELLENT:
            quality_factor = 1.0f;
            break;
        default:
            quality_factor = 0.0f;
            break;
    }
    
    // 最终权重
    return base_weight * quality_factor;
}

/**
 * @brief 判断是否为极端天气条件
 * 
 * @param temperature 温度 (°C)
 * @param wind_speed 风速 (m/s)
 * @return true 极端天气
 * @return false 正常天气
 */
static bool is_weather_extreme(float temperature, float wind_speed) {
    // 极端低温（低于-20°C）或极端大风（大于25m/s）
    return (temperature < -20.0f || wind_speed > 25.0f);
}

/**
 * @brief 根据置信度和连续有效计数确定数据质量级别
 * 
 * @param confidence 置信度（0.0-1.0）
 * @param consecutive_valid 连续有效计数
 * @return DataQuality 数据质量级别
 */
static DataQuality determine_data_quality(float confidence, int consecutive_valid) {
    // 无效数据
    if (confidence <= 0.1f || consecutive_valid <= 1) {
        return DATA_QUALITY_INVALID;
    }
    
    // 基于置信度确定质量级别
    if (confidence > 0.9f && consecutive_valid >= 10) {
        return DATA_QUALITY_EXCELLENT;
    } else if (confidence > 0.7f && consecutive_valid >= 5) {
        return DATA_QUALITY_HIGH;
    } else if (confidence > 0.4f && consecutive_valid >= 3) {
        return DATA_QUALITY_MEDIUM;
    } else {
        return DATA_QUALITY_LOW;
    }
}

/**
 * @brief 初始化数据融合模块
 * 
 * @return int 成功返回0，失败返回非0值
 */
int init_data_fusion(void) {
    // 初始化模块状态
    if (g_fusion_state.initialized) {
        log_message(LOG_LEVEL_INFO, "数据融合: 模块已经初始化");
        return 0;
    }
    
    // 初始化数据源质量
    init_data_source_quality(&g_fusion_state.sensor_quality);
    init_data_source_quality(&g_fusion_state.visual_quality);
    init_data_source_quality(&g_fusion_state.stereo_quality);
    init_data_source_quality(&g_fusion_state.weather_quality);
    
    // 初始化卡尔曼滤波器
    g_fusion_state.process_noise = DEFAULT_PROCESS_NOISE;
    g_fusion_state.measurement_noise = DEFAULT_MEASUREMENT_NOISE;
    g_fusion_state.anomaly_threshold = DEFAULT_ANOMALY_THRESHOLD;
    init_kalman_filter(&g_fusion_state.ice_thickness_filter, 
                       g_fusion_state.process_noise, 
                       g_fusion_state.measurement_noise);
    
    // 初始化滑动窗口
    init_sliding_window();
    
    // 初始化历史数据
    g_fusion_state.last_visual_thickness = 0.0f;
    g_fusion_state.last_stereo_thickness = 0.0f;
    g_fusion_state.last_sensor_tension = 0.0f;
    g_fusion_state.last_temperature = 0.0f;
    g_fusion_state.last_humidity = 0.0f;
    g_fusion_state.last_wind_speed = 0.0f;
    g_fusion_state.last_update_time = get_timestamp_ms();
    
    g_fusion_state.initialized = true;
    log_message(LOG_LEVEL_INFO, "数据融合: 模块初始化成功");
    
    return 0;
}

/**
 * @brief 重置数据融合状态
 * 
 * @return int 成功返回0，失败返回非0值
 */
int reset_data_fusion(void) {
    if (!g_fusion_state.initialized) {
        log_message(LOG_LEVEL_ERROR, "数据融合: 模块未初始化");
        return -1;
    }
    
    // 重置数据源质量
    init_data_source_quality(&g_fusion_state.sensor_quality);
    init_data_source_quality(&g_fusion_state.visual_quality);
    init_data_source_quality(&g_fusion_state.stereo_quality);
    init_data_source_quality(&g_fusion_state.weather_quality);
    
    // 重置卡尔曼滤波器
    init_kalman_filter(&g_fusion_state.ice_thickness_filter, 
                       g_fusion_state.process_noise, 
                       g_fusion_state.measurement_noise);
    
    // 重置滑动窗口
    init_sliding_window();
    
    // 重置历史数据
    g_fusion_state.last_visual_thickness = 0.0f;
    g_fusion_state.last_stereo_thickness = 0.0f;
    g_fusion_state.last_sensor_tension = 0.0f;
    g_fusion_state.last_temperature = 0.0f;
    g_fusion_state.last_humidity = 0.0f;
    g_fusion_state.last_wind_speed = 0.0f;
    g_fusion_state.last_update_time = get_timestamp_ms();
    
    log_message(LOG_LEVEL_INFO, "数据融合: 模块状态已重置");
    
    return 0;
}

/**
 * @brief 设置数据融合参数
 * 
 * @param process_noise 过程噪声协方差
 * @param measurement_noise 测量噪声协方差
 * @param anomaly_threshold 异常检测阈值
 * @return int 成功返回0，失败返回非0值
 */
int set_fusion_parameters(float process_noise, float measurement_noise, float anomaly_threshold) {
    if (!g_fusion_state.initialized) {
        log_message(LOG_LEVEL_ERROR, "数据融合: 模块未初始化");
        return -1;
    }
    
    // 参数有效性检查
    if (process_noise <= 0.0f || measurement_noise <= 0.0f || anomaly_threshold <= 0.0f) {
        log_message(LOG_LEVEL_ERROR, "数据融合: 参数无效，必须为正值");
        return -1;
    }
    
    g_fusion_state.process_noise = process_noise;
    g_fusion_state.measurement_noise = measurement_noise;
    g_fusion_state.anomaly_threshold = anomaly_threshold;
    
    // 更新卡尔曼滤波器参数
    g_fusion_state.ice_thickness_filter.Q = process_noise;
    g_fusion_state.ice_thickness_filter.R = measurement_noise;
    
    log_message(LOG_LEVEL_INFO, "数据融合: 参数已更新 (Q=%.4f, R=%.4f, 异常阈值=%.2f)", 
               process_noise, measurement_noise, anomaly_threshold);
    
    return 0;
}

/**
 * @brief 清理数据融合模块资源
 */
void cleanup_data_fusion(void) {
    if (!g_fusion_state.initialized) {
        return;
    }
    
    // 不需要特殊清理，只需重置状态标志
    g_fusion_state.initialized = false;
    
    log_message(LOG_LEVEL_INFO, "数据融合: 模块资源已清理");
}

/**
 * @brief 获取数据质量等级描述文本
 * 
 * @param quality 数据质量枚举值
 * @return const char* 质量描述文本
 */
const char* get_data_quality_description(DataQuality quality) {
    if (quality >= DATA_QUALITY_INVALID && quality <= DATA_QUALITY_EXCELLENT) {
        return data_quality_descriptions[quality];
    }
    return "未知";
}

/**
 * @brief 计算冰生长率 - 从simulation.c迁移过来的增强版本
 */
float calculate_ice_growth_rate_ex(float temperature, float humidity, float wind_speed) {
    float base_growth_rate = 0.0f;
    
    // 基础生长率，温度越低，湿度越大，生长越快
    if (temperature < 0.0f) {
        // 负温度，可能结冰
        base_growth_rate = -temperature * 0.05f;
        // 湿度影响
        base_growth_rate *= (humidity / 50.0f);
    } else {
        // 正温度，可能融化
        base_growth_rate = -temperature * 0.02f;
        // 最小为0，不会成冰
        if (base_growth_rate > 0.0f) base_growth_rate = 0.0f;
    }
    
    // 风速影响，适当风速增加结冰速度，过大风速减少
    float wind_factor;
    if (wind_speed < 10.0f) {
        wind_factor = wind_speed / 5.0f;
    } else {
        wind_factor = 2.0f - (wind_speed - 10.0f) / 10.0f;
        if (wind_factor < 0.5f) wind_factor = 0.5f;
    }
    
    base_growth_rate *= wind_factor;
    
    // 添加随机波动
    float random_factor = 1.0f + ((float)rand() / RAND_MAX) * 0.4f - 0.2f;  // 0.8到1.2之间随机波动
    
    return base_growth_rate * random_factor;
}

/**
 * @brief 计算覆冰生长速率 - 内部使用版本
 * @param temperature 环境温度(°C)
 * @param humidity 相对湿度(%)
 * @param wind_speed 风速(m/s)
 * @return 每次更新增加的覆冰厚度(mm)
 */
static float data_fusion_calculate_ice_growth_rate(float temperature, float humidity, float wind_speed) {
    // 调用本文件中的扩展版本函数，避免递归调用
    return calculate_ice_growth_rate_ex(temperature, humidity, wind_speed);
}

/**
 * @brief 计算运行时冰厚度 - 从simulation.c迁移过来的增强版本
 */
float calculate_runtime_ice_thickness_ex(float sensor_thickness, float visual_thickness, 
                                      float temperature, bool use_correction) {
    // 如果任一厚度为负值，则视为无效数据
    if (sensor_thickness < 0.0f || visual_thickness < 0.0f) {
        // 使用有效的那个数据
        if (sensor_thickness >= 0.0f) return sensor_thickness;
        if (visual_thickness >= 0.0f) return visual_thickness;
        return 0.0f;  // 两个都无效，返回0
    }
    
    // 简单加权平均，视觉数据权重高一些
    float weight_sensor = 0.4f;
    float weight_visual = 0.6f;
    
    // 如果需要修正，根据两者差异调整权重
    if (use_correction) {
        float diff = fabsf(sensor_thickness - visual_thickness);
        
        if (diff > 5.0f) {
            // 差异较大时，更偏向视觉数据
            weight_sensor = 0.2f;
            weight_visual = 0.8f;
        } else if (diff < 1.0f) {
            // 差异小时，接近平均
            weight_sensor = 0.45f;
            weight_visual = 0.55f;
        }
        
        // 温度修正
        if (temperature > 0.0f) {
            // 正温度下，视觉可能更准确(传感器可能受融水影响)
            weight_visual += 0.1f;
            weight_sensor -= 0.1f;
        } else if (temperature < -10.0f) {
            // 极低温下，传感器可能更准确
            weight_visual -= 0.1f;
            weight_sensor += 0.1f;
        }
        
        // 确保权重和为1
        float sum = weight_sensor + weight_visual;
        weight_sensor /= sum;
        weight_visual /= sum;
    }
    
    // 计算加权平均值
    return sensor_thickness * weight_sensor + visual_thickness * weight_visual;
}

/**
 * @brief 根据传感器数据计算拉力相关参数
 */
bool process_tension_data(float tension1, float tension2, 
                        float wind_speed, float wind_direction,
                        float tension_no_ice,
                        float *out_avg_tension, float *out_tension_increase) {
    // 初始化输出参数
    if (out_avg_tension) *out_avg_tension = 0.0f;
    if (out_tension_increase) *out_tension_increase = 0.0f;
    
    // 检查传感器值是否在合理范围内
    bool tension1_valid = true;
    bool tension2_valid = true;
    
    // 设置合理的拉力值范围
    const float MIN_TENSION = 100.0f;  // 最小拉力值(N)
    const float MAX_TENSION = tension_no_ice * 5.0f;  // 最大拉力值(N)
    
    // 检查传感器1拉力值
    if (tension1 < MIN_TENSION || tension1 > MAX_TENSION) {
        log_message(LOG_LEVEL_WARNING, 
                  "传感器1拉力值异常: %.2fN，超出合理范围[%.2fN, %.2fN]", 
                  tension1, MIN_TENSION, MAX_TENSION);
        tension1_valid = false;
    }
    
    // 检查传感器2拉力值
    if (tension2 < MIN_TENSION || tension2 > MAX_TENSION) {
        log_message(LOG_LEVEL_WARNING, 
                  "传感器2拉力值异常: %.2fN，超出合理范围[%.2fN, %.2fN]", 
                  tension2, MIN_TENSION, MAX_TENSION);
        tension2_valid = false;
    }
    
    // 计算拉力平均值 - 根据有效性判断使用哪些数据
    float avg_tension = 0.0f;
    
    if (!tension1_valid && !tension2_valid) {
        // 两个都无效，使用无冰状态下的拉力值作为默认值
        avg_tension = tension_no_ice;
        log_message(LOG_LEVEL_WARNING, 
                  "两个拉力传感器数据均无效，使用无冰状态下的默认拉力值: %.2fN", avg_tension);
        return false; // 表示处理失败
    } else if (tension1_valid && tension2_valid) {
        // 两个传感器都有效，检查一致性
        float diff = fabsf(tension1 - tension2);
        float avg = (tension1 + tension2) / 2.0f;
        float relative_diff = diff / avg;
        
        // 检查风速是否过大，大风会导致拉力传感器数据异常
        // 使用app_control.h中定义的常量
        bool is_high_wind = wind_speed >= HIGH_WIND_THRESHOLD;
        bool is_extreme_wind = wind_speed >= EXTREME_WIND_THRESHOLD;
        
        // 风向与导线方向的夹角影响 - 假设导线方向为0°/180°
        float wind_angle = fmodf(wind_direction, 180.0f);
        if (wind_angle > 90.0f) {
            wind_angle = 180.0f - wind_angle;
        }
        // 垂直时影响最大，平行时影响最小
        float perpendicular_factor = wind_angle / 90.0f; // 0-1，1表示完全垂直
        
        // 根据风速和风向调整可接受的差异阈值
        float acceptable_diff_threshold = 0.2f; // 默认20%
        
        if (is_extreme_wind) {
            // 极端大风情况下，调整可接受的差异阈值，更倾向于使用更可靠的数据
            acceptable_diff_threshold = 0.3f + perpendicular_factor * 0.2f; // 30-50%
            log_message(LOG_LEVEL_WARNING, 
                      "极端大风条件(%.1fm/s)可能导致拉力传感器数据不稳定，调整差异容忍度至%.1f%%", 
                      wind_speed, acceptable_diff_threshold * 100.0f);
        } else if (is_high_wind) {
            // 大风情况下，调整可接受的差异阈值
            acceptable_diff_threshold = 0.25f + perpendicular_factor * 0.15f; // 25-40%
            log_message(LOG_LEVEL_INFO, 
                      "大风条件(%.1fm/s)可能影响拉力传感器数据，调整差异容忍度至%.1f%%", 
                      wind_speed, acceptable_diff_threshold * 100.0f);
        }
        
        if (relative_diff > acceptable_diff_threshold) {
            // 差异超过阈值，认为异常
            log_message(LOG_LEVEL_WARNING, 
                      "两个拉力传感器数据差异过大: %.2fN vs %.2fN (%.1f%%)，使用更可靠的数据",
                      tension1, tension2, relative_diff*100.0f);
            
            // 风速大且风向垂直于导线时，传感器数据可靠性分析
            if (is_high_wind && perpendicular_factor > 0.7f) {
                // 当大风垂直吹向导线时，风上方的传感器数据可能更可靠
                // 假设传感器1在风上方（这里简化处理，实际应根据真实安装位置确定）
                log_message(LOG_LEVEL_INFO, 
                          "大风垂直吹向导线(%.1fm/s, %.1f°)，优先考虑风上方传感器数据", 
                          wind_speed, wind_direction);
                
                // 这里可以根据实际安装位置确定哪个传感器在风上方
                // 这里仅为示例，假设传感器1更可靠
                avg_tension = tension1;
                log_message(LOG_LEVEL_INFO, "选择传感器1的拉力值: %.2fN", avg_tension);
            } else {
                // 否则使用更接近上次测量值的数据
                if (fabsf(tension1 - tension_no_ice) < 
                    fabsf(tension2 - tension_no_ice)) {
                    avg_tension = tension1;
                    log_message(LOG_LEVEL_INFO, "选择传感器1的拉力值: %.2fN", avg_tension);
                } else {
                    avg_tension = tension2;
                    log_message(LOG_LEVEL_INFO, "选择传感器2的拉力值: %.2fN", avg_tension);
                }
            }
        } else {
            // 差异在合理范围内，使用平均值
            // 大风条件下，根据风向可能需要对拉力值进行修正
            if (is_high_wind && perpendicular_factor > 0.5f) {
                // 大风垂直吹向导线时，风力会直接影响测量的拉力值
                // 计算风力修正系数 - 采用更科学的风力修正方式
                float wind_correction = 1.0f;
                
                // 只有当风速足够大并且有显著的垂直分量时才进行修正
                if (wind_speed > HIGH_WIND_THRESHOLD && perpendicular_factor > 0.3f) {
                    // 风力修正比例与风速和垂直度共同相关
                    // 1. 风速越大，修正越大
                    // 2. 风向越垂直于导线，修正越大
                    // 3. 考虑导线类型和安装张力的影响
                    
                    // 基于风速的修正因子
                    float wind_speed_factor = (wind_speed - HIGH_WIND_THRESHOLD) / 30.0f;
                    wind_speed_factor = fminf(wind_speed_factor, 0.25f); // 最大修正25%
                    
                    // 基于垂直度的修正因子
                    float angle_factor = powf(perpendicular_factor, 1.5f); // 使用指数函数强调垂直影响
                    
                    // 计算总修正系数
                    wind_correction = 1.0f - wind_speed_factor * angle_factor;
                }
                
                // 应用风力修正
                avg_tension = (tension1 + tension2) / 2.0f * wind_correction;
                log_message(LOG_LEVEL_INFO, 
                          "应用风力修正: 原始平均拉力=%.2fN, 修正系数=%.2f, 修正后拉力=%.2fN", 
                          (tension1 + tension2) / 2.0f, wind_correction, avg_tension);
            } else {
                // 正常情况下使用简单平均值
                avg_tension = (tension1 + tension2) / 2.0f;
            }
        }
    } else {
        // 只有一个传感器有效
        avg_tension = tension1_valid ? tension1 : tension2;
        log_message(LOG_LEVEL_INFO, "使用有效的传感器数据: %.2fN", avg_tension);
    }
    
    // 计算拉力增加量
    float tension_increase = avg_tension - tension_no_ice;
    
    // 设置输出参数
    if (out_avg_tension) *out_avg_tension = avg_tension;
    if (out_tension_increase) *out_tension_increase = tension_increase;
    
    return tension_increase > 0.0f; // 只有当拉力增加量为正时才认为有覆冰
}

/**
 * @brief 融合传感器和视觉数据以计算等值线缆覆冰厚度
 */
float fuse_equivalent_cable_data(float sensor_thickness, float visual_thickness,
                               float temperature, float humidity,
                               float wind_speed, float wind_direction,
                               bool use_correction) {
    // 非负检查
    if (sensor_thickness < 0.0f) sensor_thickness = 0.0f;
    if (visual_thickness < 0.0f) visual_thickness = 0.0f;
    
    // 如果某一数据显著高于另一数据，使用较高可信度的数据
    if (sensor_thickness > visual_thickness * 3.0f && sensor_thickness > 5.0f) {
        log_message(LOG_LEVEL_INFO, "传感器厚度(%.2fmm)远大于视觉厚度(%.2fmm)，使用中间值", 
                   sensor_thickness, visual_thickness);
        return (sensor_thickness * 0.6f + visual_thickness * 0.4f);
    }
    
    if (visual_thickness > sensor_thickness * 3.0f && visual_thickness > 5.0f) {
        log_message(LOG_LEVEL_INFO, "视觉厚度(%.2fmm)远大于传感器厚度(%.2fmm)，使用中间值", 
                   visual_thickness, sensor_thickness);
        return (visual_thickness * 0.6f + sensor_thickness * 0.4f);
    }
    
    // 如果两个数据都很小（接近0），考虑无覆冰情况
    if (sensor_thickness < 0.5f && visual_thickness < 0.5f) {
        // 两者都接近0，认为没有覆冰
        return 0.0f;
    }
    
    // 基本加权融合
    float fused_thickness = calculate_runtime_ice_thickness_ex(
        sensor_thickness, visual_thickness, temperature, use_correction);
    
    // 应用环境修正
    // 只在两者都有有效数据时应用修正
    if (sensor_thickness > 0.0f && visual_thickness > 0.0f) {
        // 计算环境修正系数
        float environment_correction = 1.0f;
        
        // 特殊条件处理
        // 例如：极低温高湿度条件下，冰的生长更快
        if (temperature < -10.0f && humidity > 85.0f) {
            environment_correction = 1.05f;
            log_message(LOG_LEVEL_DEBUG, "极低温高湿度条件，应用修正系数: %.2f", environment_correction);
        }
        // 其他环境条件的修正...
        
        // 应用修正
        fused_thickness *= environment_correction;
    }
    
    // 确保最终厚度在合理范围内
    if (fused_thickness < 0.0f) fused_thickness = 0.0f;
    if (fused_thickness > 50.0f) {
        log_message(LOG_LEVEL_WARNING, "计算的覆冰厚度超过最大阈值50mm，将限制为50mm");
        fused_thickness = 50.0f;
    }
    
    // 更新历史数据
    g_fusion_state.last_visual_thickness = visual_thickness;
    
    return fused_thickness;
}

/**
 * @brief 融合传感器和双目视觉数据以计算真实线缆覆冰厚度
 */
float fuse_real_cable_data(float stereo_thickness, bool sensor_available,
                         float temperature, float humidity,
                         float wind_speed, float wind_direction) {
    // 非负检查
    if (stereo_thickness < 0.0f) stereo_thickness = 0.0f;
    
    // 应用修正
    float corrected_thickness = stereo_thickness;
    
    // 如果有传感器数据，可以应用更复杂的修正
    if (sensor_available) {
        // 基于环境参数的修正系数
        float temp_factor = 1.0f;
        float humid_factor = 1.0f;
        float wind_factor = 1.0f;
        
        // 温度修正
        if (temperature < -5.0f) {
            // 低温可能导致更厚的冰
            temp_factor = 1.0f + ((-5.0f - temperature) / 15.0f) * 0.2f;
        } else if (temperature > 0.0f) {
            // 正温条件下冰可能减少
            temp_factor = 1.0f - (temperature / 10.0f) * 0.5f;
            if (temp_factor < 0.5f) temp_factor = 0.5f; // 至少保留50%
        }
        
        // 湿度修正
        if (humidity > 85.0f) {
            // 高湿可能导致更厚的冰
            humid_factor = 1.0f + ((humidity - 85.0f) / 15.0f) * 0.1f;
        } else if (humidity < 60.0f) {
            // 低湿条件下冰生长减缓
            humid_factor = 1.0f - ((60.0f - humidity) / 30.0f) * 0.2f;
            if (humid_factor < 0.8f) humid_factor = 0.8f; // 至少保留80%
        }
        
        // 风速修正
        if (wind_speed > 3.0f && wind_speed < 10.0f) {
            // 适度风速有利于覆冰形成
            wind_factor = 1.0f + ((wind_speed - 3.0f) / 7.0f) * 0.1f;
        } else if (wind_speed > 10.0f) {
            // 强风可能导致一些冰脱落
            wind_factor = 1.0f - ((wind_speed - 10.0f) / 20.0f) * 0.2f;
            if (wind_factor < 0.8f) wind_factor = 0.8f; // 至少保留80%
        }
        
        // 计算总修正系数
        float total_factor = temp_factor * humid_factor * wind_factor;
        total_factor = fminf(fmaxf(total_factor, 0.5f), 1.5f); // 限制在0.5-1.5范围内
        
        // 应用修正
        corrected_thickness *= total_factor;
    }
    
    // 确保修正后的厚度在合理范围内
    if (corrected_thickness < 0.0f) corrected_thickness = 0.0f;
    if (corrected_thickness > 50.0f) corrected_thickness = 50.0f;
    
    // 更新历史数据
    g_fusion_state.last_stereo_thickness = corrected_thickness;
    
    return corrected_thickness;
}

/**
 * @brief 根据多源数据计算覆冰厚度
 * 
 * @param sensor_data 传感器数据包指针，可为NULL
 * @param visual_data 视觉数据指针，可为NULL
 * @param stereo_data 立体视觉数据指针，可为NULL
 * @param available_data 可用数据源标志位
 * @param cable_params 线缆参数指针
 * @param run_mode 运行模式
 * @param fusion_result 融合结果输出
 * @return bool 成功返回true，失败返回false
 */
bool fuse_all_data(
    const SensorDataPacket *sensor_data,
    const VisualIceData *visual_data,
    const StereoData *stereo_data,
    int available_data,
    const CableParameters *cable_params,
    RunMode run_mode,
    FusionData *fusion_result
) {
    if (!fusion_result || !cable_params) {
        log_message(LOG_LEVEL_ERROR, "数据融合: 无效的参数");
        return false;
    }
    
    // 创建ExtendedFusionData结构体用于内部处理
    ExtendedFusionData extended_result;
    memset(&extended_result, 0, sizeof(ExtendedFusionData));
    
    // 调用扩展函数进行完整计算
    bool success = fuse_all_data_extended(
        sensor_data,
        visual_data,
        stereo_data,
        available_data,
        cable_params,
        run_mode,
        fusion_result,
        &extended_result
    );
    
    // 返回处理结果
    return success;
}

/**
 * @brief 根据多源数据计算覆冰厚度和扩展信息
 * 
 * @param sensor_data 传感器数据包指针，可为NULL
 * @param visual_data 视觉数据指针，可为NULL
 * @param stereo_data 立体视觉数据指针，可为NULL
 * @param available_data 可用数据源标志位
 * @param cable_params 线缆参数指针
 * @param run_mode 运行模式
 * @param fusion_result 基础融合结果输出
 * @param extended_result 扩展融合结果输出，可为NULL
 * @return bool 成功返回true，失败返回false
 */
bool fuse_all_data_extended(
    const SensorDataPacket *sensor_data,
    const VisualIceData *visual_data,
    const StereoData *stereo_data,
    int available_data,
    const CableParameters *cable_params,
    RunMode run_mode,
    FusionData *fusion_result,
    ExtendedFusionData *extended_result
) {
    // 参数检查
    if (!fusion_result) {
        return false;
    }
    
    // 初始化融合结果
    memset(fusion_result, 0, sizeof(FusionData));
    if (extended_result) {
        memset(extended_result, 0, sizeof(ExtendedFusionData));
    }
    
    // 初始化异常标志
    uint32_t anomaly_flags = 0;
    
    // 如果没有可用数据，返回失败
    if (available_data == 0) {
        log_message(LOG_LEVEL_WARNING, "数据融合: 没有可用数据");
        return false;
    }
    
    // 获取当前时间
    uint64_t current_time = get_timestamp_ms();
    
    // 检查数据超时
    if ((available_data & SENSOR_DATA_AVAILABLE) && 
        (current_time - sensor_data->timestamp > MAX_DATA_AGE)) {
        anomaly_flags |= ANOMALY_SENSOR_TIMEOUT;
        log_message(LOG_LEVEL_WARNING, "数据融合: 传感器数据超时");
    }
    
    // 处理传感器数据
    if ((available_data & SENSOR_DATA_AVAILABLE) && sensor_data) {
        // 更新传感器数据
        float sensor_tension = (sensor_data->tension1 + sensor_data->tension2) / 2.0f;
        fusion_result->sensor_tension = sensor_tension;
        fusion_result->temperature = sensor_data->temperature;
        fusion_result->humidity = sensor_data->humidity;
        fusion_result->wind_speed = sensor_data->wind_speed;
        fusion_result->wind_direction = sensor_data->wind_direction;
        
        // 检查传感器数据异常
        bool sensor_anomaly = detect_anomaly(
            sensor_tension, 
            g_fusion_state.last_sensor_tension, 
            g_fusion_state.sensor_quality.variance, 
            g_fusion_state.anomaly_threshold
        );
        
        if (sensor_anomaly) {
            log_message(LOG_LEVEL_WARNING, "数据融合: 检测到传感器数据异常");
            anomaly_flags |= ANOMALY_SENSOR_JUMP;
        }
        
        // 更新传感器数据质量
        update_data_quality(
            &g_fusion_state.sensor_quality, 
            sensor_tension, 
            g_fusion_state.last_sensor_tension, 
            current_time
        );
        
        // 更新气象数据质量
        update_data_quality(
            &g_fusion_state.weather_quality, 
            sensor_data->temperature, 
            g_fusion_state.last_temperature, 
            current_time
        );
        
        // 更新滑动窗口
        update_sliding_window(SENSOR_DATA_AVAILABLE, sensor_tension);
        
        // 检查极端天气条件
        if (is_weather_extreme(sensor_data->temperature, sensor_data->wind_speed)) {
            log_message(LOG_LEVEL_WARNING, "数据融合: 检测到极端天气条件");
            anomaly_flags |= ANOMALY_WEATHER_EXTREME;
        }
        
        // 更新历史数据
        g_fusion_state.last_sensor_tension = sensor_tension;
        g_fusion_state.last_temperature = sensor_data->temperature;
        g_fusion_state.last_humidity = sensor_data->humidity;
        g_fusion_state.last_wind_speed = sensor_data->wind_speed;
    } else {
        // 无传感器数据时，复用上次的气象数据
        fusion_result->temperature = g_fusion_state.last_temperature;
        fusion_result->humidity = g_fusion_state.last_humidity;
        fusion_result->wind_speed = g_fusion_state.last_wind_speed;
    }
    
    // 处理视觉数据
    if ((available_data & VISUAL_DATA_AVAILABLE) && visual_data) {
        // 更新视觉厚度
        fusion_result->visual_thickness = visual_data->thickness;
        
        // 检查视觉数据异常
        bool visual_anomaly = detect_anomaly(
            visual_data->thickness, 
            g_fusion_state.last_visual_thickness, 
            g_fusion_state.visual_quality.variance, 
            g_fusion_state.anomaly_threshold
        );
        
        if (visual_anomaly) {
            log_message(LOG_LEVEL_WARNING, "数据融合: 检测到视觉数据异常");
            anomaly_flags |= ANOMALY_VISUAL_JUMP;
        }
        
        // 更新视觉数据质量
        update_data_quality(
            &g_fusion_state.visual_quality, 
            visual_data->thickness, 
            g_fusion_state.last_visual_thickness, 
            current_time
        );
        
        // 更新滑动窗口
        update_sliding_window(VISUAL_DATA_AVAILABLE, visual_data->thickness);
        
        // 更新历史数据
        g_fusion_state.last_visual_thickness = visual_data->thickness;
    }
    
    // 处理立体视觉数据
    if ((available_data & STEREO_DATA_AVAILABLE) && stereo_data) {
        // 更新立体视觉厚度
        fusion_result->stereo_thickness = stereo_data->thickness;
        
        // 检查立体视觉数据异常
        bool stereo_anomaly = detect_anomaly(
            stereo_data->thickness, 
            g_fusion_state.last_stereo_thickness, 
            g_fusion_state.stereo_quality.variance, 
            g_fusion_state.anomaly_threshold
        );
        
        if (stereo_anomaly) {
            log_message(LOG_LEVEL_WARNING, "数据融合: 检测到立体视觉数据异常");
            anomaly_flags |= ANOMALY_STEREO_JUMP;
        }
        
        // 更新立体视觉数据质量
        update_data_quality(
            &g_fusion_state.stereo_quality, 
            stereo_data->thickness, 
            g_fusion_state.last_stereo_thickness, 
            current_time
        );
        
        // 更新滑动窗口
        update_sliding_window(STEREO_DATA_AVAILABLE, stereo_data->thickness);
        
        // 更新历史数据
        g_fusion_state.last_stereo_thickness = stereo_data->thickness;
    }
    
    // 将异常检测结果存储到扩展结构体中
    if (extended_result) {
        extended_result->has_anomaly = (anomaly_flags != 0);
        extended_result->anomaly_flags = anomaly_flags;
    }
    
    // 基于可用数据源计算覆冰厚度
    float estimated_thickness = 0.0f;
    float raw_thickness = 0.0f;
    float total_weight = 0.0f;
    
    // 计算各数据源权重
    float sensor_weight = 0.0f;
    float visual_weight = 0.0f;
    float stereo_weight = 0.0f;
    
    // 基于数据质量和时间自适应计算权重
    if (available_data & SENSOR_DATA_AVAILABLE) {
        sensor_weight = calculate_fusion_weight(
            &g_fusion_state.sensor_quality, 
            g_program_start_time, 
            current_time
        );
    }
    
    if (available_data & VISUAL_DATA_AVAILABLE) {
        visual_weight = calculate_fusion_weight(
            &g_fusion_state.visual_quality, 
            g_program_start_time, 
            current_time
        );
    }
    
    if (available_data & STEREO_DATA_AVAILABLE) {
        stereo_weight = calculate_fusion_weight(
            &g_fusion_state.stereo_quality, 
            g_program_start_time, 
            current_time
        );
    }
    
    // 保存权重到扩展结果
    if (extended_result) {
        extended_result->sensor_weight = sensor_weight;
        extended_result->visual_weight = visual_weight;
        extended_result->stereo_weight = stereo_weight;
    }
    
    // 如果所有权重都很低，增加最可靠数据源的权重
    float max_weight = fmaxf(sensor_weight, fmaxf(visual_weight, stereo_weight));
    if (max_weight < 0.3f && available_data != 0) {
        if (max_weight == sensor_weight && (available_data & SENSOR_DATA_AVAILABLE)) {
            sensor_weight = 0.5f;
        } else if (max_weight == visual_weight && (available_data & VISUAL_DATA_AVAILABLE)) {
            visual_weight = 0.5f;
        } else if (max_weight == stereo_weight && (available_data & STEREO_DATA_AVAILABLE)) {
            stereo_weight = 0.5f;
        }
    }
    
    // 检查数据一致性
    if ((available_data & VISUAL_DATA_AVAILABLE) && (available_data & STEREO_DATA_AVAILABLE)) {
        float diff = fabsf(fusion_result->visual_thickness - fusion_result->stereo_thickness);
        if (diff > 3.0f * g_fusion_state.anomaly_threshold) {
            log_message(LOG_LEVEL_WARNING, "数据融合: 视觉与立体视觉数据冲突");
            anomaly_flags |= ANOMALY_DATA_CONFLICT;
            
            // 冲突时选择质量更高的数据源
            if (g_fusion_state.visual_quality.quality > g_fusion_state.stereo_quality.quality) {
                visual_weight *= 1.5f;
                stereo_weight *= 0.5f;
            } else {
                visual_weight *= 0.5f;
                stereo_weight *= 1.5f;
            }
        }
    }
    
    // 计算传感器估计的覆冰厚度
    float sensor_thickness = 0.0f;
    if ((available_data & SENSOR_DATA_AVAILABLE) && cable_params->tension_no_ice > 0.0f) {
        // 基于拉力计算覆冰厚度
        float excess_tension = fusion_result->sensor_tension - cable_params->tension_no_ice;
        if (excess_tension > 0.0f) {
            // 使用简化的物理模型估计厚度，参数可能需要调整
            sensor_thickness = 0.1f * excess_tension;  // 拉力转换系数需要标定
        }
    }
    
    // 加权融合计算厚度，不同模式使用不同的策略
    switch (run_mode) {
        case RUN_MODE_SENSOR_ONLY:
            // 仅使用传感器数据
            if (available_data & SENSOR_DATA_AVAILABLE) {
                raw_thickness = sensor_thickness;
                total_weight = sensor_weight;
            }
            break;
            
        case RUN_MODE_SENSOR_VISUAL:
            // 使用传感器+视觉数据
            if ((available_data & SENSOR_DATA_AVAILABLE) && sensor_weight > 0.0f) {
                raw_thickness += sensor_weight * sensor_thickness;
                total_weight += sensor_weight;
            }
            if ((available_data & VISUAL_DATA_AVAILABLE) && visual_weight > 0.0f) {
                raw_thickness += visual_weight * fusion_result->visual_thickness;
                total_weight += visual_weight;
            }
            break;
            
        case RUN_MODE_STEREO_CAMERA:
            // 使用立体视觉数据为主
            if ((available_data & STEREO_DATA_AVAILABLE) && stereo_weight > 0.0f) {
                raw_thickness = fusion_result->stereo_thickness;
                total_weight = 1.0f;  // 完全使用立体视觉
            }
            break;
            
        case RUN_MODE_FULL_SYSTEM:
        case RUN_MODE_SIMULATION:
            // 使用所有可用数据，权重加权
            if ((available_data & SENSOR_DATA_AVAILABLE) && sensor_weight > 0.0f) {
                raw_thickness += sensor_weight * sensor_thickness;
                total_weight += sensor_weight;
            }
            if ((available_data & VISUAL_DATA_AVAILABLE) && visual_weight > 0.0f) {
                raw_thickness += visual_weight * fusion_result->visual_thickness;
                total_weight += visual_weight;
            }
            if ((available_data & STEREO_DATA_AVAILABLE) && stereo_weight > 0.0f) {
                raw_thickness += stereo_weight * fusion_result->stereo_thickness;
                total_weight += stereo_weight;
            }
            break;
            
        default:
            // 默认使用所有可用数据，权重加权
            if ((available_data & SENSOR_DATA_AVAILABLE) && sensor_weight > 0.0f) {
                raw_thickness += sensor_weight * sensor_thickness;
                total_weight += sensor_weight;
            }
            if ((available_data & VISUAL_DATA_AVAILABLE) && visual_weight > 0.0f) {
                raw_thickness += visual_weight * fusion_result->visual_thickness;
                total_weight += visual_weight;
            }
            if ((available_data & STEREO_DATA_AVAILABLE) && stereo_weight > 0.0f) {
                raw_thickness += stereo_weight * fusion_result->stereo_thickness;
                total_weight += stereo_weight;
            }
            break;
    }
    
    // 归一化厚度（如果有有效权重）
    if (total_weight > 0.0f) {
        raw_thickness /= total_weight;
    } else {
        // 如果没有有效权重，使用历史数据
        raw_thickness = g_current_ice_thickness;
    }
    
    // 保存原始厚度到扩展结果
    if (extended_result) {
        extended_result->raw_thickness = raw_thickness;
    }
    
    // 应用卡尔曼滤波进行平滑
    if (raw_thickness >= 0.0f) {
        if (extended_result) {
            extended_result->raw_thickness = raw_thickness;
            extended_result->is_filtered = true;
        }
        estimated_thickness = apply_kalman_filter(&g_fusion_state.ice_thickness_filter, raw_thickness);
    } else {
        estimated_thickness = raw_thickness;
        if (extended_result) {
            extended_result->is_filtered = false;
        }
    }
    
    // 更新全局当前覆冰厚度
    g_current_ice_thickness = estimated_thickness;
    
    // 计算其他覆冰相关参数
    if (estimated_thickness > 0.0f) {
        // 计算冰密度 - 可根据温度、湿度调整
        fusion_result->ice_density = calculate_ice_density(
            fusion_result->temperature, 
            fusion_result->humidity
        );
        
        // 计算冰体积和重量
        calculate_ice_metrics(
            estimated_thickness,
            cable_params->diameter,
            fusion_result->ice_density,
            &fusion_result->ice_volume,
            &fusion_result->ice_weight
        );
        
        // 计算理论拉力
        fusion_result->calc_tension = calculate_ice_tension(
            fusion_result->ice_weight,
            cable_params->span
        );
    }
    
    // 确定覆冰数据质量
    DataQuality overall_quality = DATA_QUALITY_INVALID;
    if (available_data & STEREO_DATA_AVAILABLE) {
        overall_quality = g_fusion_state.stereo_quality.quality;
    } else if (available_data & VISUAL_DATA_AVAILABLE) {
        overall_quality = g_fusion_state.visual_quality.quality;
    } else if (available_data & SENSOR_DATA_AVAILABLE) {
        overall_quality = g_fusion_state.sensor_quality.quality;
    }
    
    // 根据异常降低质量
    if (anomaly_flags != 0) {
        if (overall_quality > DATA_QUALITY_LOW) {
            overall_quality--;
        }
    }
    
    // 保存数据质量到扩展结果
    if (extended_result) {
        extended_result->ice_data_quality = overall_quality;
        extended_result->weather_data_quality = g_fusion_state.weather_quality.quality;
    }
    
    // 计算融合结果置信度
    float overall_confidence = 0.0f;
    if (total_weight > 0.0f) {
        if (available_data & STEREO_DATA_AVAILABLE) {
            overall_confidence += stereo_weight * g_fusion_state.stereo_quality.confidence / total_weight;
        }
        if (available_data & VISUAL_DATA_AVAILABLE) {
            overall_confidence += visual_weight * g_fusion_state.visual_quality.confidence / total_weight;
        }
        if (available_data & SENSOR_DATA_AVAILABLE) {
            overall_confidence += sensor_weight * g_fusion_state.sensor_quality.confidence / total_weight;
        }
    }
    
    // 如果有异常，降低置信度
    if (anomaly_flags != 0) {
        overall_confidence *= 0.7f;
    }
    
    // 保存置信度到扩展结果
    if (extended_result) {
        extended_result->fusion_confidence = overall_confidence;
    }
    
    // 计算风险等级
    fusion_result->risk_level = calculate_risk_level(
        estimated_thickness, 
        fusion_result->ice_weight, 
        fusion_result->wind_speed
    );
    
    // 更新最后更新时间
    g_fusion_state.last_update_time = current_time;
    
    // 记录详细日志（非模拟模式下）
    if (run_mode != RUN_MODE_SIMULATION) {
        char quality_desc[128];
        snprintf(quality_desc, sizeof(quality_desc), 
                "传感器:%s 视觉:%s 立体:%s 置信度:%.2f",
                get_data_quality_description(g_fusion_state.sensor_quality.quality),
                get_data_quality_description(g_fusion_state.visual_quality.quality),
                get_data_quality_description(g_fusion_state.stereo_quality.quality),
                extended_result ? extended_result->fusion_confidence : 0.0f);
                
        log_message(LOG_LEVEL_DEBUG, "数据融合: 权重(传感器=%.2f 视觉=%.2f 立体=%.2f) 厚度=%.2fmm %s %s",
                   sensor_weight, visual_weight, stereo_weight, estimated_thickness, 
                   (extended_result && extended_result->has_anomaly) ? "存在异常" : "正常",
                   quality_desc);
    }
    
    // 如果需要，将扩展结果的基础部分复制回基本结果
    if (extended_result) {
        extended_result->base = *fusion_result;
    }
    
    return true;
}

/**
 * @brief 使用传感器数据和视觉数据计算融合冰厚度
 */
float calculate_fusion_ice_thickness(SensorData *sensor_data, VisualIceData *visual_data) {
    if (!sensor_data || !visual_data) {
        return 0.0f;
    }
    
    // 获取各自的厚度值
    float sensor_thickness = 0.0f;
    
    // 从传感器拉力计算厚度，拉力增加量决定是否有覆冰
    if (sensor_data->tension1 > 0 && sensor_data->tension2 > 0) {
        float avg_tension = (sensor_data->tension1 + sensor_data->tension2) / 2.0f;
        float base_tension = 1000.0f; // 假设基础拉力为1000N
        
        // 只有当拉力大于基础拉力时才认为有覆冰
        if (avg_tension > base_tension) {
            float tension_increase = avg_tension - base_tension;
            // 假设每10N拉力对应1mm冰厚度
            sensor_thickness = tension_increase / 10.0f;
        }
    }
    
    float visual_thickness = visual_data->thickness;
    
    // 确保视觉厚度不为负
    if (visual_thickness < 0.0f) {
        visual_thickness = 0.0f;
    }
    
    // 使用已有的函数计算融合厚度
    return fuse_equivalent_cable_data(
        sensor_thickness, 
        visual_thickness,
        sensor_data->temperature,
        sensor_data->humidity,
        sensor_data->wind_speed,
        sensor_data->wind_direction,
        true  // 使用修正
    );
}

/**
 * @brief 计算覆冰的体积和重量
 * 
 * @param thickness 冰厚度(mm)
 * @param cable_diameter 缆线直径(mm)
 * @param ice_density 冰密度(kg/m³)
 * @param out_volume 输出冰体积(mm³/mm)
 * @param out_weight 输出冰重量(kg/m)
 */
static void calculate_ice_metrics(float thickness, float cable_diameter, float ice_density,
                               float *out_volume, float *out_weight) {
    if (thickness <= 0.0f || cable_diameter <= 0.0f) {
        // 没有覆冰
        if (out_volume) *out_volume = 0.0f;
        if (out_weight) *out_weight = 0.0f;
        return;
    }
    
    // 计算等效覆冰体积 (mm³/mm)
    // 公式：π*((d+2t)²-d²)/4，其中d是缆线直径，t是冰厚度
    float total_diameter = cable_diameter + 2 * thickness;
    float volume = (M_PI * (total_diameter * total_diameter - cable_diameter * cable_diameter)) / 4.0f;
    
    if (out_volume) *out_volume = volume;
    
    // 计算每米重量 (kg/m)
    // 公式：体积(mm³/mm) * 密度(kg/m³) * 10^-6 (转换mm³到m³)
    float weight = volume * ice_density * 1.0e-6f;
    
    if (out_weight) *out_weight = weight;
}

/**
 * @brief 根据覆冰重量和跨度计算理论拉力增加量
 * 
 * @param ice_weight 冰重量(kg/m)
 * @param span 跨度(m)
 * @return float 拉力增加量(N)
 */
static float calculate_ice_tension(float ice_weight, float span) {
    // 简化模型：假设冰均匀分布在导线上，使用抛物线模型
    // 拉力增加量 = 冰重量 * 重力加速度 * 跨度 / 8 * 最大弧垂比例
    const float GRAVITY = 9.81f; // 重力加速度(m/s²)
    const float SAG_RATIO = 0.05f; // 最大弧垂与跨度的比例(估计值)
    
    return ice_weight * GRAVITY * span * SAG_RATIO;
}

/**
 * @brief 计算覆冰风险等级
 * 
 * @param thickness 冰厚度(mm)
 * @param ice_weight 冰重量(kg/m)
 * @param wind_speed 风速(m/s)
 * @return RiskLevel 风险等级
 */
static RiskLevel calculate_risk_level(float thickness, float ice_weight, float wind_speed) {
    // 基于厚度的风险评估
    RiskLevel thickness_risk = RISK_LEVEL_NONE;
    
    if (thickness < 5.0f) {
        thickness_risk = RISK_LEVEL_NONE;
    } else if (thickness < 10.0f) {
        thickness_risk = RISK_LEVEL_LOW;
    } else if (thickness < 20.0f) {
        thickness_risk = RISK_LEVEL_MEDIUM;
    } else if (thickness < 30.0f) {
        thickness_risk = RISK_LEVEL_HIGH;
    } else {
        thickness_risk = RISK_LEVEL_EXTREME;
    }
    
    // 基于重量的风险评估
    RiskLevel weight_risk = RISK_LEVEL_NONE;
    
    if (ice_weight < 1.0f) {
        weight_risk = RISK_LEVEL_NONE;
    } else if (ice_weight < 3.0f) {
        weight_risk = RISK_LEVEL_LOW;
    } else if (ice_weight < 6.0f) {
        weight_risk = RISK_LEVEL_MEDIUM;
    } else if (ice_weight < 10.0f) {
        weight_risk = RISK_LEVEL_HIGH;
    } else {
        weight_risk = RISK_LEVEL_EXTREME;
    }
    
    // 考虑风速的影响
    // 在大风条件下，较低厚度和重量的覆冰也可能产生高风险
    if (wind_speed > EXTREME_WIND_THRESHOLD) {
        // 极端大风条件下，提高风险等级
        if (thickness_risk < RISK_LEVEL_EXTREME) thickness_risk++; 
        if (weight_risk < RISK_LEVEL_EXTREME) weight_risk++;
    } else if (wind_speed > HIGH_WIND_THRESHOLD) {
        // 大风条件下，轻微提高风险等级
        if (thickness_risk < RISK_LEVEL_EXTREME && thickness > 5.0f) thickness_risk++;
        if (weight_risk < RISK_LEVEL_EXTREME && ice_weight > 1.0f) weight_risk++;
    }
    
    // 取两种风险评估的最大值
    return (thickness_risk > weight_risk) ? thickness_risk : weight_risk;
} 